import pygame
from pygame.locals import *
import robo3pi as s
import serial
import time
from pygame.tests.base_test import pygame_quit
 
COM_NUM = '7'
ID = '2'
 
pygame.init()
s.init('com' + COM_NUM)
screen = pygame.display.set_mode((640, 480))
pygame.display.set_caption('Polulu Racer')
pygame.mouse.set_visible(0)
 
 
done = False

up_is_pressed = False;
down_is_pressed = False;
right_is_pressed = False;
left_is_pressed = False;

while not done:
    event = pygame.event.poll()
    if not (event.type == pygame.NOEVENT):
        
        if (event.type == KEYDOWN):
            if (event.key == K_UP):
                up_is_pressed = True;
                if (down_is_pressed):
                    s.stop(ID)
                elif (left_is_pressed):
                    s.move(ID,100,150)
                elif (right_is_pressed):
                    s.move(ID,150,100)
                else:
                    s.move(ID, 150, 150)
                    
            elif (event.key == K_DOWN):
                down_is_pressed = True;
                if (up_is_pressed):
                    s.stop(ID)
                elif (left_is_pressed):
                    s.move(ID,-100,-150)
                elif (right_is_pressed):
                    s.move(ID,-150,-100)
                else:
                    s.move(ID, -150, -150)
        
            elif (event.key == K_LEFT):
                left_is_pressed = True
                if (right_is_pressed):
                    s.stop(ID)
                elif (up_is_pressed):
                    s.move(ID,100,150)
                elif (down_is_pressed):
                    s.move(ID,-100,-150)
                else:
                    s.move(ID, 0, 90)
                    
            elif (event.key == K_RIGHT):
                right_is_pressed = True
                if (left_is_pressed):
                    s.stop(ID)
                elif (up_is_pressed):
                    s.move(ID,150,100)
                elif (down_is_pressed):
                    s.move(ID,-150,-100)
                else:
                    s.move(ID, 90, 0)
            
        elif (event.type == KEYUP):
            if (event.key == K_UP):
                up_is_pressed = False;
                if (down_is_pressed):
                    s.move(ID, -150, -150)
                elif (left_is_pressed):
                    s.move(ID,0,90)
                elif (right_is_pressed):
                    s.move(ID,90,0)
                else:
                    s.stop(ID)
                    
            elif (event.key == K_DOWN):
                down_is_pressed = False
                if (up_is_pressed):
                    s.move(ID, 150, 150)
                elif (left_is_pressed):
                    s.move(ID, 0, 90)
                elif (right_is_pressed):
                    s.move(ID, 90, 0)
                else:
                    s.stop(ID)
            
            elif (event.key == K_LEFT):
                left_is_pressed = False
                if (up_is_pressed):
                    s.move(ID, 150, 150)
                elif (down_is_pressed):
                    s.move(ID, -150, -150)
                elif (right_is_pressed):
                    s.move(ID, 90, 0)
                else:
                    s.stop(ID)
            
            elif (event.key == K_RIGHT):
                right_is_pressed = False
                if (up_is_pressed):
                    s.move(ID, 150, 150)
                elif (down_is_pressed):
                    s.move(ID, -150, -150)
                elif (left_is_pressed):
                    s.move(ID, 0, 90)
                else:
                    s.stop(ID)
    
        '''Exit:'''
        if (event.type == KEYDOWN and event.key == K_ESCAPE):
            s.stop(ID)
            s.close()
            done = True
    
    time.sleep(0.15)
          
pygame.quit()